#ifndef CAMODOCAL_H
#define CAMODOCAL_H

#include <Eigen/Eigen>
#include <Eigen/StdVector>
#include <Eigen/Geometry>

namespace Camodo {

template<typename T>
Eigen::Matrix< T,4,4 > QuaternionMultMatLeft(const Eigen::Quaternion< T >& q )
{
    return (Eigen::Matrix< T, 4,4>() << q.w(), -q.z(), q.y(), q.x(),
                                                                          q.z(), q.w(), -q.x(), q.y(),
                                                                          -q.y(), q.x(), q.w(), q.z(),
                                                                          -q.x(), -q.y(), -q.z(), q.w()).finished();
}

template< typename T>
Eigen::Matrix< T ,4,4 > QuaternionMultMatRight(const Eigen::Quaternion< T >& q )
{
  return (Eigen::Matrix<T ,4,4>() <<q.w(), q.z(), -q.y(), q.x(),
                                                                      -q.z(), q.w(), q.x(), q.y(),
                                                                      q.y(), -q.x(), q.w(), q.z(),
                                                                      -q.x(), -q.y(), -q.z(), q.w()).finished();

}

/*
 *  Rotation matrix to euler
*/
template<typename T>
void mat2RPY(const Eigen::Matrix<T, 3, 3>& m, T& roll, T& pitch, T& yaw)
{
    roll = atan2(m(2,1), m(2,2));
    pitch = atan2(-m(2,0), sqrt(m(2,1) * m(2,1) + m(2,2) * m(2,2)));
    yaw = atan2(m(1,0), m(0,0));
}

class CamOdoCalibration
{

public:
    bool estimate(const std::vector<Eigen::Quaterniond, Eigen::aligned_allocator<Eigen::Quaterniond> > &quats_odo,
                                        const std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &tvecs_odo,
                                        const std::vector<Eigen::Quaterniond, Eigen::aligned_allocator<Eigen::Quaterniond> > &quats_cam,
                                        const std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &tvecs_cam,
                                        Eigen::Matrix4d& Trc);
private:

  void refineEstimate(Eigen::Matrix4d & Trc, double scale,
                      const std::vector < Eigen::Quaterniond, Eigen::aligned_allocator < Eigen::Quaterniond > >& quats_odo,
                      const std::vector < Eigen::Vector3d, Eigen::aligned_allocator < Eigen::Vector3d > >& tvecs_odo,
                      const std::vector < Eigen::Quaterniond, Eigen::aligned_allocator < Eigen::Quaterniond > >& quats_cam,
                      const std::vector < Eigen::Vector3d, Eigen::aligned_allocator < Eigen::Vector3d > >& tvecs_cam);

  bool estimateRyx( const std::vector < Eigen::Quaterniond, Eigen::aligned_allocator < Eigen::Quaterniond > >& quats_odo,
                    const std::vector < Eigen::Vector3d, Eigen::aligned_allocator < Eigen::Vector3d > >& tvecs_odo,
                    const std::vector < Eigen::Quaterniond, Eigen::aligned_allocator < Eigen::Quaterniond > >& quats_cam,
                    const std::vector < Eigen::Vector3d, Eigen::aligned_allocator < Eigen::Vector3d > >& tvecs_cam,
                    Eigen::Matrix3d& R_yx);

  bool SolveConstraintqyx(const Eigen::Vector4d t1, const Eigen::Vector4d t2, double &x1, double &x2);
};

}
#endif // CAMODOCAL_H
